/**
 * @file algo_Filter.c
 * @brief Software implementation of matrix for 51 microcontroller.
 * @author David Xu
 * @version 1.0.0
 * @date 2023-11-21
 * @note none
 */

#include "algo_matrix.h"

#include "drv_Log.h"



/*============================================================================*/
void mat_zero(Matrix_t *r) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = 0;    // the matrix that will be set to zero.
        }
    }
}

void qFP16_mat_zero(Matrix_fp16_t *r) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = 0;    // the matrix that will be set to zero.
        }
    }
}
/*============================================================================*/

void mat_setup(Matrix_t *r, int value) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; ++i) {
        for (j = 0; j < MATRIX_COLUMN; ++j) {
            r->Mdat[i][j] = value;
        }
    }
}

void qFP16_mat_setup(Matrix_fp16_t *r, float value) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; ++i) {
        for (j = 0; j < MATRIX_COLUMN; ++j) {
            r->Mdat[i][j] = qFP16_FloatToFP(value);
        }
    }
}
/*============================================================================*/
void mat_copy(Matrix_t *r, Matrix_t *a) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = a->Mdat[i][j];
        }
    }
}

void qFP16_mat_copy(Matrix_fp16_t *r, Matrix_fp16_t *a) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = a->Mdat[i][j];
        }
    }
}

/*============================================================================*/
void mat_add(Matrix_t *r, Matrix_t *a, Matrix_t *b) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = a->Mdat[i][j] + b->Mdat[i][j];
        }
    }
}

void qFP16_mat_add(Matrix_fp16_t *r, Matrix_fp16_t *a, Matrix_fp16_t *b) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = qFP16_Add(a->Mdat[i][j], b->Mdat[i][j]);
        }
    }
}


/*============================================================================*/

void mat_sub(Matrix_t *r, Matrix_t *a, Matrix_t *b) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = a->Mdat[i][j] - b->Mdat[i][j];
        }
    }
}

void qFP16_mat_sub(Matrix_fp16_t *r, Matrix_fp16_t *a, Matrix_fp16_t *b) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = qFP16_Sub(a->Mdat[i][j], b->Mdat[i][j]);
        }
    }
}

/*============================================================================*/

void mat_scal_mult(Matrix_t *r, Matrix_t *a, int k) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = k * a->Mdat[i][j];
        }
    }
}


void qFP16_mat_scal_mult(Matrix_fp16_t *r, Matrix_fp16_t *a, float k) {
    unsigned char i, j;
    qFP16_t       k_fp16 = qFP16_FloatToFP(k);
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = qFP16_Mul(k_fp16, a->Mdat[i][j]);
        }
    }
}

/*============================================================================*/

void mat_mult(Matrix_t *r, Matrix_t *a, Matrix_t *b) {
    unsigned char i, j, k;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = 0;
            for (k = 0; k < MATRIX_COLUMN; k++) {
                r->Mdat[i][j] += a->Mdat[i][k] * b->Mdat[k][j];
            }
        }
    }
}

void qFP16_mat_mult(Matrix_fp16_t *r, Matrix_fp16_t *a, Matrix_fp16_t *b) {
    unsigned char i, j, k;
    qFP16_t       tmp;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            r->Mdat[i][j] = 0;
            for (k = 0; k < MATRIX_COLUMN; k++) {
                tmp           = qFP16_Mul(a->Mdat[i][k], b->Mdat[k][j]);
                r->Mdat[i][j] = qFP16_Add(r->Mdat[i][j], tmp);
            }
        }
    }
}

/*============================================================================*/

void mat_transpose(Matrix_t *result, Matrix_t *input) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            result->Mdat[i][j] = input->Mdat[j][i];
        }
    }
}

void qFP16_mat_transpose(Matrix_fp16_t *result, Matrix_fp16_t *input) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            result->Mdat[i][j] = input->Mdat[j][i];
        }
    }
}


/*============================================================================*/

void mat_identity(Matrix_t *result) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            if (i == j) {
                result->Mdat[i][j] = 1;
            } else {
                result->Mdat[i][j] = 0;
            }
        }
    }
}


void qFP16_mat_identity(Matrix_fp16_t *result) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            if (i == j) {
                result->Mdat[i][j] = qFP16_Constant(1);
            } else {
                result->Mdat[i][j] = 0;
            }
        }
    }
}


/*============================================================================*/
#define MATRIX_SCALE_FACTOR 1000    // Define a scale factor for fixed-point representation
void mat_inverse(Matrix_t *result, Matrix_t *input) {
    int32_t det = input->Mdat[0][0] * input->Mdat[1][1] - input->Mdat[0][1] * input->Mdat[1][0];

    if (det != 0) {
        // Calculate the inverse using fixed-point arithmetic
        result->Mdat[0][0] = (input->Mdat[1][1] * MATRIX_SCALE_FACTOR) / det;
        result->Mdat[0][1] = (-input->Mdat[0][1] * MATRIX_SCALE_FACTOR) / det;
        result->Mdat[1][0] = (-input->Mdat[1][0] * MATRIX_SCALE_FACTOR) / det;
        result->Mdat[1][1] = (input->Mdat[0][0] * MATRIX_SCALE_FACTOR) / det;
    } else {
        // Handle error for non-invertible matrix
        printf("Matrix is not invertible.\n");
        // You could also set result to NULL or to an identity matrix, depending on your error handling strategy    }
    }
}


/**
 * @brief Calculates the inverse of a matrix using fixed-point arithmetic.
 *
 * This function calculates the inverse of a given matrix using fixed-point arithmetic. If the matrix is not
 * invertible, the function does nothing.
 *
 * @param result Pointer to the resultant matrix.
 * @param input Pointer to the input matrix.
 */
void mat_inverse_fp16(Matrix_t *result, Matrix_t *input) {
    qFP16_t       fp_inv_det, fp_det;
    Matrix_fp16_t fp_temp, fp_result;
    int32_t       det = input->Mdat[0][0] * input->Mdat[1][1] - input->Mdat[0][1] * input->Mdat[1][0];
    //
    fp_temp.Mdat[0][0] = qFP16_IntToFP(input->Mdat[0][0]);
    fp_temp.Mdat[0][1] = qFP16_IntToFP(input->Mdat[0][1]);
    fp_temp.Mdat[1][0] = qFP16_IntToFP(input->Mdat[1][0]);
    fp_temp.Mdat[1][1] = qFP16_IntToFP(input->Mdat[1][1]);

    if (det != 0 && det <= qFP16_Constant(1)) {
        fp_det     = qFP16_IntToFP(det);
        fp_inv_det = qFP16_Div(qFP16_Constant(1), fp_det);

        fp_result.Mdat[0][0] = qFP16_Mul(fp_temp.Mdat[1][1], fp_inv_det);
        fp_result.Mdat[0][1] = qFP16_Mul(-fp_temp.Mdat[0][1], fp_inv_det);
        fp_result.Mdat[1][0] = qFP16_Mul(-fp_temp.Mdat[1][0], fp_inv_det);
        fp_result.Mdat[1][1] = qFP16_Mul(fp_temp.Mdat[0][0], fp_inv_det);
        //
        result->Mdat[0][0] = qFP16_FPToInt(fp_result.Mdat[0][0]);
        result->Mdat[0][1] = qFP16_FPToInt(fp_result.Mdat[0][1]);
        result->Mdat[1][0] = qFP16_FPToInt(fp_result.Mdat[1][0]);
        result->Mdat[1][1] = qFP16_FPToInt(fp_result.Mdat[1][1]);

    } else {
        // Handle error for non-invertible matrix
        printf("Matrix is not invertible.\n");
        // You could also set result to NULL or to an identity matrix, depending on your error handling strategy
    }
}


void qFP16_mat_inverse(Matrix_fp16_t *result, Matrix_fp16_t *input) {
    qFP16_t fp_inv_det;
    // det = input->Mdat[0][0] * input->Mdat[1][1] - input->Mdat[0][1] * input->Mdat[1][0];
    qFP16_t det = qFP16_Sub(qFP16_Mul(input->Mdat[0][0], input->Mdat[1][1]), qFP16_Mul(input->Mdat[0][1], input->Mdat[1][0]));

    if (det != 0) {
        fp_inv_det = qFP16_Div(qFP16_Constant(1), det);

        result->Mdat[0][0] = qFP16_Mul(input->Mdat[1][1], fp_inv_det);
        result->Mdat[0][1] = qFP16_Mul(-input->Mdat[0][1], fp_inv_det);
        result->Mdat[1][0] = qFP16_Mul(-input->Mdat[1][0], fp_inv_det);
        result->Mdat[1][1] = qFP16_Mul(input->Mdat[0][0], fp_inv_det);
    } else {
        // Handle error for non-invertible matrix
        printf("Matrix is not invertible.\n");
        // You could also set result to NULL or to an identity matrix, depending on your error handling strategy
    }
}



/*============================================================================*/

void array_to_matrix(Matrix_t *mat, const float *array) {
    int i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            mat->Mdat[i][j] = array[i * MATRIX_COLUMN + j];
        }
    }
}

void qFP16_array_to_matrix(Matrix_fp16_t *mat, const float *array) {
    int i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            mat->Mdat[i][j] = qFP16_FloatToFP(array[i * MATRIX_COLUMN + j]);
        }
    }
}

/*============================================================================*/

void mat_print(Matrix_t *m) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            printf("Matrix: %ld\n ", m->Mdat[i][j]);
        }
        printf("\n");
    }
}

void qFP16_mat_print(Matrix_fp16_t *m) {
    unsigned char i, j;
    for (i = 0; i < MATRIX_ROW; i++) {
        for (j = 0; j < MATRIX_COLUMN; j++) {
            printf("FP_Matrix: %ld\n ", m->Mdat[i][j]);
        }
        printf("\n");
    }
}



#if 0
// 测试程序
void matrix_test(void) {
    Matrix_t a, b, result;
    float    array[MATRIX_ROW * MATRIX_COLUMN] = { -1, 2, -3, 4 };

    // 初始化矩阵a和b
    // 直接赋值
    a.Mdat[0][0] = 1;
    a.Mdat[0][1] = 2;
    a.Mdat[1][0] = 3;
    a.Mdat[1][1] = 4;
    // mat_setup(&a, 1);
    // mat_setup(&b, 1);
    b.Mdat[0][0] = 5;
    b.Mdat[0][1] = 6;
    b.Mdat[1][0] = 7;
    b.Mdat[1][1] = 8;

    // 测试矩阵加法
    printf("Testing matrix addition:\n");
    mat_add(&result, &a, &b);
    mat_print(&result);

    // 测试矩阵减法
    printf("\nTesting matrix subtraction:\n");
    mat_sub(&result, &a, &b);
    mat_print(&result);

    // 测试矩阵的标量乘法
    printf("\nTesting matrix scalar multiplication:\n");
    mat_scal_mult(&result, &a, 10);
    mat_print(&result);

    // 测试矩阵乘法
    printf("\nTesting matrix multiplication:\n");
    mat_mult(&result, &a, &b);
    mat_print(&result);

    // 测试矩阵转置
    printf("Testing matrix transpose:\n");
    mat_transpose(&result, &a);
    mat_print(&result);

    // 测试创建单位矩阵
    printf("\nTesting creating identity matrix:\n");
    mat_identity(&result);
    mat_print(&result);

    // 测试计算矩阵的逆
    printf("\nTesting matrix inverse:\n");
    mat_inverse(&result, &a);
    mat_print(&result);

    // 测试使用定点算法计算矩阵的逆
    printf("\nTesting matrix inverse with fixed-point arithmetic:\n");
    mat_inverse_fp16(&result, &a);
    mat_print(&result);

    // 测试将一维数组转换为矩阵
    printf("\nTesting converting array to matrix:\n");
    array_to_matrix(&result, array);
    mat_print(&result);
    nop;
}

#endif


#if 0
// 测试程序
void matrix_test(void) {
    Matrix_fp16_t a, b, result;
    float         array[MATRIX_ROW * MATRIX_COLUMN] = { -1, 2, -3, 4 };

    // 初始化矩阵a和b
    // 直接赋值
    // qFP16_mat_setup(&a, 1);
    // qFP16_mat_setup(&b, 1);
    a.Mdat[0][0] = qFP16_Constant(1);
    a.Mdat[0][1] = qFP16_Constant(2);
    a.Mdat[1][0] = qFP16_Constant(3);
    a.Mdat[1][1] = qFP16_Constant(4);



    b.Mdat[0][0] = qFP16_Constant(5);
    b.Mdat[0][1] = qFP16_Constant(6);
    b.Mdat[1][0] = qFP16_Constant(7);
    b.Mdat[1][1] = qFP16_Constant(8);

    // 测试矩阵加法
    printf("Testing matrix addition:\n");
    qFP16_mat_add(&result, &a, &b);
    qFP16_mat_print(&result);

    // 测试矩阵减法
    printf("\nTesting matrix subtraction:\n");
    qFP16_mat_sub(&result, &a, &b);
    qFP16_mat_print(&result);

    // 测试矩阵的标量乘法
    printf("\nTesting matrix scalar multiplication:\n");
    qFP16_mat_scal_mult(&result, &a, 10);
    qFP16_mat_print(&result);

    // 测试矩阵乘法
    printf("\nTesting matrix multiplication:\n");
    qFP16_mat_mult(&result, &a, &b);
    qFP16_mat_print(&result);

    // 测试矩阵转置
    printf("Testing matrix transpose:\n");
    qFP16_mat_transpose(&result, &a);
    qFP16_mat_print(&result);

    // 测试创建单位矩阵
    printf("\nTesting creating identity matrix:\n");
    qFP16_mat_identity(&result);
    qFP16_mat_print(&result);

    // 测试计算矩阵的逆
    printf("\nTesting matrix inverse:\n");
    qFP16_mat_inverse(&result, &a);
    qFP16_mat_print(&result);



    // 测试将一维数组转换为矩阵
    printf("\nTesting converting array to matrix:\n");
    qFP16_array_to_matrix(&result, array);
    qFP16_mat_print(&result);
    nop;
}

#endif